/*
 *  robot_real_fpga.h
 *  
 *
 *  Created by Pallab Datta on 09/23/09.
 *  Copyright 2009 __The Neurosciences Institute__. All rights reserved.
 *
 *
 *  Wrapper for the robot code.
 *
 */

#include <iostream>
#include <nsi/thinklink/config.h>
#include <nsi/thinklink/remote_robot.h>
#include <nsi/thinklink/device.h>
#include <nsi/thinklink/error.h>
#include <stdio.h>
#include <stdlib.h>
#include <nsi/thinklink/stl.h>
#include <nsi/thinklink/scoped_ptr.h>
#include <nsi/thinklink/serial_device.h>
#include <nsi/thinklink/socket_device.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <string>
#include <stdexcept>
#include <unistd.h>
#include <getopt.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include "local_options.h"


using ::nsi::thinklink::stl::runtime_error;
using ::nsi::thinklink::serial_device;
using ::nsi::thinklink::socket_device;

using namespace std;

const	double	pi=3.1415926;
const int frame_time_ms = 20;
const int n_motors=3;
const int MAX_MOTORS = 50;
const float max_velocity_rad_s = 114.0f/60.0*2*pi; 
const float rad_per_level = 300.0f/1024.0f*pi/180.0f;


extern MPI_Comm network_comm;
extern int myid;

#define APEX3

#ifdef APEX1  // (yellow wiring harness.)

#define ELBOW 3
#define DELTOID 5
#define SHOULDER 6
// shoulder, deltoid, elbow
const float mins[]={200, 380, 305};
const float maxs[]={512, 657, 687};

#else // (blue wiring harness.)

#define HAND 0
#define FOREARM 8
#define ELBOW 7
#define DELTOID 9
#define SHOULDER 10
// shoulder, deltoid, elbow
const float mins[]={185,204,404};
const float maxs[]={492,497,643};

#endif

const float range_rad[] = { (maxs[0]-mins[0])*rad_per_level, (maxs[1]-mins[1])*rad_per_level,(maxs[2]-mins[2])*rad_per_level};
const float home_pose_rad[] = { range_rad[0], range_rad[1], 0};


template <typename T>
T clip(T x, T minx, T maxx){
	
	return min( max(x,minx) , maxx);
}


struct example_robot_client
	: public ::nsi::thinklink::remote_robot<>
{
  // Types
  typedef ::nsi::thinklink::remote_robot<> robot_type;
  typedef robot_type::actuator_type actuator_type;
  typedef ::nsi::thinklink::device device;

  example_robot_client(device* link = 0)
    : robot_type(link),
      time_(2),
      single_(true)
  {

  }

  void random_positions()
  {
    for (int i = 0; i < robot_type::actuators_length; ++i)
      random_position(this->actuators_[i]);
    
    this->burst_write();
    sleep();
  }

  void random_single_positions()
  {
    for (int i = 0; i < robot_type::actuators_length; ++i)
    {
      random_position(this->actuators_[i]);
      this->burst_write();
      sleep();
    }
  }

  void random_position(actuator_type& act)
  {
    act.position = rand() % act.position.max();
    act.speed = rand() % act.speed.max();
    act.torque = rand() % act.torque.max();
  }

  void run()
  {
    // Populate initial position
     this->burst_query();
 
    for (int i =0; i < 1000; ++i)
      if (single_)
        random_single_positions();
      else
        random_positions();
  }

  
  void sleep()
  {
    if (time_ > 0)
      ::sleep(time_);
  }
  
  void reset()
  {
    this->link_reset();
  }
  
  void initializer(::nsi::thinklink::device* link)
  {
    this->link(link);
    using ::nsi::thinklink::actuator;
    
    this->initialize();
   
    /* 
    for (int i = 0; i < actuators_length; ++i)
    {
      actuator_type& act = actuators_[i];
      act.margin_cw = 16;
      act.margin_ccw = 16;
      act.slope_cw = 32;
      act.slope_ccw = 32;
    }
	
	  burst_write();
    */
    
    burst_query();

    for (int i = 0; i < actuators_length; ++i)
    {
      actuator_type& act = actuators_[i];
      act.position = act.present_position;
      act.margin_cw = 1;
      act.margin_ccw = 1;
      act.slope_cw = 32;
      act.slope_ccw = 32;
      act.torque = act.torque.max() / 4;
      act.speed = act.speed.max() / 4;
    }

    burst_write();

    /*
    this->run();
    for (int i = 0; i < 4; ++i)
    {

            fprintf(stderr, "init %d\n", i);
            try
            {
                    ::sleep(6); 
                    actuators_[3].position = actuators_[3].position + 25;
                    burst_write();
            }
            catch (std::exception& ex)
            {
                    fprintf(stderr, "error %s\n", ex.what());
                    throw;
            }
    }
    */

    //this->actuators().iterate(&actuator_type::synchronize_partial);
    
    //sleep();
    
    fprintf(stderr, "initialize %p %p\n", this->link(), link);
  }

  void set_position(int servo, uint16_t pos, uint16_t speed, uint16_t torque)
  {
    actuator_type& act = this->actuators_[servo];

    fprintf(stderr, "----- set pos %d: %u %u %u\n", servo, act.position.value(), act.speed.value(), act.torque.value());
    
    act.position = pos;
    act.speed = speed;
    act.torque = torque;

    fprintf(stderr, "----- set to pos %d: %u %u %u\n", servo, pos, speed, torque);
    
    //sleep();
  }


  int get_position(int servo)
  {
    actuator_type& act = this->actuators_[servo];
    return act.present_position;
  }

  int get_speed(int servo)
  {
    actuator_type& act = this->actuators_[servo];
    return act.present_speed;
  }

  int get_torque(int servo)
  {
    actuator_type& act = this->actuators_[servo];
    return act.present_torque;
  }

  void do_state_conversions()
  { 
    const float rpm_to_rad_s = 1.0f/60.0f*2*pi;

     // positions
    for( int i = 0; i < n_motors;i++){
      state_array[i] = rad_per_level* (state_array[i]-mins[i]);
    }
     // velocities can be negative or positive.
    for( int i = n_motors; i < 2*n_motors;i++){
      state_array[i] = state_array[i] * .111 * rpm_to_rad_s;
    }
     // torques can be negative or positive.
    for( int i = 2*n_motors; i < 3*n_motors;i++){
      state_array[i] = state_array[i] * (1.0/1024.0);
    }
  }

  void get_arm_proprioception(float &shoulder, float &deltoid, float &elbow, float &sdot, float &ddot, float &edot, float &storque, float &dtorque, float &etorque, int t)
  {
    static bool first_call = true;

    if( t % frame_time_ms == 0 || first_call ){
      first_call = false; // just in case it's the first time.

      if( myid == 0 ){

		  cout << "************************************";
        burst_query();
        
        state_array[0] = get_position(SHOULDER);
        state_array[1] = get_position(DELTOID);
        state_array[2] = get_position(ELBOW);

        state_array[3] = get_speed(SHOULDER);
        state_array[4] = get_speed(DELTOID);
        state_array[5] = get_speed(ELBOW);

        state_array[6] = get_torque(SHOULDER);
        state_array[7] = get_torque(DELTOID);
        state_array[8] = get_torque(ELBOW);

        do_state_conversions();

      }
    }
    //MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm); // Proc 0 owns the data.
    shoulder = state_array[0];
    deltoid = state_array[1];
    elbow = state_array[2];
    sdot = state_array[3];
    ddot = state_array[4];
    edot = state_array[5];
    storque = state_array[6];
    dtorque = state_array[7];
    etorque = state_array[8];
  }

  void set_arm_command(float shoulder, float deltoid, float elbow, float sdot, float ddot, float edot, int t, int motor_update_interval_ms, int motor_update_offset_ms)
  {	
    const int max_commanded_velocity = 100;
    const int torque_limit=512;
    const float levels_per_radian=1.0f/rad_per_level;
    const float rad_s_to_rpm = 60.0f/(2*pi);

    if( t % motor_update_interval_ms == motor_update_offset_ms )
    {

      if( myid == 0 )
      {

        shoulder = clip( shoulder*levels_per_radian+mins[0],mins[0],maxs[0]);
        deltoid  = clip( deltoid*levels_per_radian+mins[1],mins[1],maxs[1]);
        elbow    = clip( elbow*levels_per_radian+mins[2],mins[2],maxs[2]);
        
        set_position(SHOULDER, shoulder, clip((int)(rad_s_to_rpm * 9.0090f * sdot), 1, max_commanded_velocity), 850);
        set_position(DELTOID, deltoid, clip((int)(rad_s_to_rpm * 9.0090f * ddot), 1, max_commanded_velocity), 850);
        set_position(ELBOW, elbow, clip((int)(rad_s_to_rpm * 9.0090f * edot), 1, max_commanded_velocity), torque_limit);    
	      set_position(FOREARM, 542, clip((int)(rad_s_to_rpm * 9.0090f * 1), 1, max_commanded_velocity), torque_limit); 
        burst_write();
      }
    }
  }

protected:

  int time_;
  bool single_;
  float state_array[3*MAX_MOTORS];  

};


    
